#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"

/*
    需求1：换算出 turtle1 相对于 turtle2 的关系 
    需求2：计算角速度和线速度并发布 
        
*/
int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "sub_tf");

    ros::NodeHandle nh;

    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 1000);
    ros::Rate rate(10);

    while(ros::ok())
    {
        try
        {
            // Time(0) 取最近两帧
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2", "turtle1",ros::Time(0));
            ROS_INFO("turtle1 相对于 turtle2的消息：父级：%s, 子级:%s 偏移量(%.2f, %.2f, %.2f)", 
                    tfs.header.frame_id.c_str(),    // 预期结果 ：turrle2
                    tfs.child_frame_id.c_str(),     // 预期结果 ：turrle1
                    tfs.transform.translation.x,
                    tfs.transform.translation.y,
                    tfs.transform.translation.z
                    );
            geometry_msgs::Twist twist; 
        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("错误提示：%s", e.what());
        }

        rate.sleep();

        ros::spinOnce();        
    }
    return 0;
}
